#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include "Elevator.h"

static void defaultAction(void* object)
{
	// Count illegal calls, or something
}

static void closeDoorAction(void* object)
{
	Elevator *elevator = (Elevator*)object;
	measureWeight(elevator->weightSensor);
	// 1800kg is the max weight. And as long as it above this number it will not close
	while (elevator->weightSensor->weight > 1800)
		measureWeight(elevator->weightSensor);

	while(doorCanClose(elevator->doorSensor) != 1)
	{
		// Loop de loop till the door can close.
		// Perhaps you could sign a signal by using a mailbox
	}
}

static void openDoorAction(void* object)
{
	// Change the state of the door

	// Let people in
	Sleep(3000);
}

static void movingAction(void* object)
{
	Element *holdingElement, *currentElement;
	Destination *destination;
	Elevator *elevator = (Elevator*)object;

	listPrepareForLooping(elevator->destinations);
	holdingElement = currentElement = listNextElement(elevator->destinations);
	destination = (Destination*)holdingElement->value;

	if ((int)destination->priority == 0)
	{
		// So we don't have a urgent call, let's see which destination is most close by
		while ((currentElement = listNextElement(elevator->destinations)) != NULL)
		{
			int currentFloor =((Destination*)currentElement->value)->floor;
			int holdingFloor = ((Destination*)holdingElement->value)->floor;

			int currentDestinationDifference = elevator->currentFloor < currentFloor 
				? currentFloor - elevator->currentFloor 
				: elevator->currentFloor - currentFloor;

			int holdingDestinationDifference = elevator->currentFloor < holdingFloor
				? holdingFloor - elevator->currentFloor
				: elevator->currentFloor - holdingFloor;

 			if(currentDestinationDifference < holdingDestinationDifference)
				holdingElement = currentElement;
		}

		destination = (Destination*)holdingElement->value;
	}

	while (elevator->currentFloor != destination->floor)
	{
		// Perhaps put current floor to a mailbox?
		if(elevator->currentFloor < destination->floor)
			elevator->currentFloor++;
		else
			elevator->currentFloor--;

		Sleep(1000);
	}

	// Remove the current destination from the list
	listDeleteElement(elevator->destinations, holdingElement);
}

static void idleAction(void* object)
{
	Elevator *elevator = (Elevator*)object;

	if(elevator->destinations != NULL && elevator->destinations->firstElement != NULL)
		elevatorSendEvent(elevator, nextDestination);
}

static unsigned __stdcall elevatorTask(void* arg)
{
	task *controller = (task*)arg;
	Elevator *elevator = *((Elevator**)getArgument_task(controller));

	elevatorEvent currentEvent;
	actionType currentAction;

	while(!isTerminated_task(controller)){
		get_mailBox(&(elevator->elevatorMailbox), &currentEvent);

		elevator->status = (elevatorStatus) lookUp_STD(elevator->stateMachine, elevator->status, currentEvent, &currentAction);
		
		//Voer actie uit
		currentAction(elevator);
	}

	return 0;
}

Elevator* elevatorConstruct()
{
	Elevator *elevator;
	elevator = (Elevator*) malloc(sizeof(Elevator));

	elevator->weightSensor = weightSensorConstruct();
	elevator->doorSensor = doorSensorConstruct();

	STD *stateMachine;
	stateMachine = (STD*) malloc(sizeof(STD));

	create_STD(stateMachine, 5, defaultAction);

	// TODO: LINK TRANSITIONS IN ACTIONS
	addTransition_STD(stateMachine, idle, nextDestination, doorClosed, closeDoorAction);
	addTransition_STD(stateMachine, doorClosed, move, moving, movingAction);
	addTransition_STD(stateMachine, moving, stopping, doorOpen, openDoorAction);
	addTransition_STD(stateMachine, doorOpen, closingDoor, idle, idleAction);
	addTransition_STD(stateMachine, idle, openingDoor, doorOpen, openDoorAction);

	elevator->stateMachine = stateMachine;

	create_mailBox(&(elevator->elevatorMailbox), 10, sizeof(elevatorEvent));
	elevator->elevatorController = (task*) malloc(sizeof(task));
	create_task(elevator->elevatorController, elevatorTask, &elevator, sizeof(Elevator), 0);

	elevator->destinations = NULL;
	elevator->currentFloor = 0;
	elevator->status = idle;

	return elevator;
}

void elevatorAddDestination(Elevator *elevator, Destination *destination)
{
	if(elevator->destinations == NULL)
		elevator->destinations = listConstruct(elementConstruct(&destination));
	else
	{
		if((int)destination->priority == 1)
			listAddElementToFront(elevator->destinations, elementConstruct(&destination));
		else
			listAddElement(elevator->destinations, elementConstruct(&destination));
	}
}

void elevatorDeconstruct(Elevator *elevator)
{
	free(elevator->stateMachine);
	if(elevator->destinations != NULL)
		listDeconstruct(elevator->destinations);
	
	doorSensorDeconstruct(elevator->doorSensor);
	weightSensorDeconstruct(elevator->weightSensor);
	free(elevator);
}

void elevatorSendEvent(Elevator *elevator, elevatorEvent eventToExecute)
{
	put_mailBox(&(elevator->elevatorMailbox), &eventToExecute);
}